`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2024/08/10 23:28:01
// Design Name: 
// Module Name: position_pid
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////


module position_pid#(
    parameter               INTEGRAL_VALUE_MAX  = $signed(32'd1500)   ,  //积分的最大值
    parameter               INTEGRAL_VALUE_MIN  = $signed(-32'd1500)  ,  //积分的最小值
    parameter               FEEDBACK_VALUE_MAX  = $signed(32'd4500)   ,  //反馈的最大值
    parameter               FEEDBACK_VALUE_MIN  = $signed(0)             //反馈的最小值
)(
    input                   sys_clk_i           ,
    input                   sys_rst_n_i         ,
    
    input                   position_pid_en_i   ,
    output                  position_pid_ack_o  ,

    input signed[31:0]      P_i                 ,
    input signed[31:0]      I_i                 ,
    input signed[31:0]      D_i                 ,

    input signed[31:0]      desired_value_i     ,
    input signed[31:0]      actual_value_i      ,

    output reg signed[31:0] feedback_value_o    
);



reg[5:0]    cnt;
reg         dealing;


reg signed[31:0]    error;
reg signed[31:0]    error_sum;

reg signed[63:0]    P_fb;
reg signed[63:0]    I_fb;

reg signed[31:0]    feedback_value_d0;


assign position_pid_ack_o = ( cnt == 'd15 ) ? 1'b1 : 1'b0;

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        dealing <= 1'b0;
    else if( position_pid_ack_o == 1'b1 )
        dealing <= 1'b0;
    else if( position_pid_en_i == 1'b1 )
        dealing <= 1'b1;
    else
        dealing <= dealing;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        cnt <= 'd0;
    else if( dealing == 1'b1 )
        cnt <= cnt + 1'b1;
    else
        cnt <= 'd0;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        error <= 'd0;
    else if( cnt == 'd1 )
        error <= desired_value_i - actual_value_i;
    else
        error <= error;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        P_fb <= 'd0;
    else if( cnt == 'd3 )
        P_fb <= error * P_i;
    else if( cnt == 'd5)
        P_fb <= P_fb >>> 16;
    else
        P_fb <= P_fb;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        error_sum <= 'd0;
    else if( cnt == 'd3 )
            error_sum <= error_sum + error;
    else if( cnt == 'd5 ) begin
        if( error_sum >= INTEGRAL_VALUE_MAX ) 
            error_sum <=  INTEGRAL_VALUE_MAX;
        else if( error_sum <= INTEGRAL_VALUE_MIN)
            error_sum <= INTEGRAL_VALUE_MIN;
        else
            error_sum <= error_sum;
    end
    else
        error_sum <= error_sum;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        I_fb <= 'd0;
    else if( cnt == 'd7 )
        I_fb <= error_sum * I_i;
    else if( cnt == 'd9 )
        I_fb <= I_fb >>> 16;
    else
        I_fb <= I_fb;
end


always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        feedback_value_d0 <= 'd0;
    else if( cnt == 'd11 )
        feedback_value_d0 <= P_fb + I_fb;
    else
        feedback_value_d0 <= feedback_value_d0;
end

always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        feedback_value_o <= 'd0;
    else if( cnt == 'd13 )
        if( feedback_value_d0 > FEEDBACK_VALUE_MAX )
            feedback_value_o <= FEEDBACK_VALUE_MAX;
        else if( feedback_value_d0 < FEEDBACK_VALUE_MIN )
            feedback_value_o <= FEEDBACK_VALUE_MIN;
        else
            feedback_value_o <= feedback_value_d0;
    else
        feedback_value_o <= feedback_value_o;
end


endmodule


